package es.uji.viselab.visualservoing;

import javax.swing.JPanel;

import org.jscience.mathematics.number.Float64;
import org.jscience.mathematics.vector.Float64Matrix;
import org.jscience.mathematics.vector.Matrix;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Vector2;

public class IBVS extends VisualServoingTask {

	
	public IBVS(ExperimentParameters ep) {
		super(ep);
	}

	@Override
	public double[] compute(ImageSource image) {
		
		 Vector2[] targetCoordinateList;
		 Vector2[] imageCoordinateList;

		
		targetCoordinateList = image.getTargetCoordinateList();
		imageCoordinateList = image.getImageCoordinateList();

		int numPoints = imageCoordinateList.length;

		double jacobianDepth = 	0.5;

		// J is a matrix of 2*numPoints rows and 6 columns
		double[][] J = getJacobian(imageCoordinateList, jacobianDepth);
		// Error is a matrix of 2 rows (x,y) and numPoints columns
		double[][] error = getError(targetCoordinateList, imageCoordinateList);

		// Now, tError has numPoints rows and 2 columns
		double[][] tError = getTranspose(error);
		// The iJ has 6 rows and 2*numPoints columns
		double[][] iJ = getPseudoInverse(J);

		// tErrorVector will have 2*numPoints rows and 1 column
		double[][] tErrorVector;
		tErrorVector = new double[2 * numPoints][1];
		int index = 0;
		// The error matrix (ex1,ey1)(ex2,ey2)(e..)
		// must be all in the same column (ex1, ey2, ex2, ey2, e...)
		for (int i = 0; i < numPoints; i++) {
			tErrorVector[index][0] = tError[i][0];
			tErrorVector[index + 1][0] = tError[i][1];
			index = index + 2;
		}
		
		double sumError=0;
		for (int i=0; i<numPoints;i++){
			sumError=sumError + Math.sqrt(Math.pow(targetCoordinateList[i].x-imageCoordinateList[i].x,2)+Math.pow(targetCoordinateList[i].y-imageCoordinateList[i].y,2));
		}
		System.out.println("Error: " + sumError);
		
		
		// Velocities will have 6 rows and 1 column
		double fd =  ep.getCamParameters().getFocalDistanceCorrected();
		double[][] v = mult(iJ, tErrorVector);
		double[] velocitiesRow = { v[0][0]*jacobianDepth/fd,v[1][0]*jacobianDepth/fd,v[2][0]/10,v[3][0],v[4][0],v[5][0]};
		System.out.println("IBVS Velocities: " + velocitiesRow[0] + ", "+ velocitiesRow[1] + ", "+ velocitiesRow[2] );
        return velocitiesRow;
        
	}

	private double[][] mult(double[][] A, double[][] B) {
		// As we multiply A*B it will have:
		int rows = A.length;
		int columns = B[0].length;
		int items = A[0].length;
		if (A[0].length!=B.length)
			System.out.println("A*B; A has " + A[0].length + " columns and B has " + B.length + " rows ");

		double[][] R = new double[rows][columns];
		for (int i = 0; i < rows; i++)
			for (int j = 0; j < columns; j++) {
				R[i][j] = 0;
				for (int k = 0; k < items; k++)
					R[i][j] = R[i][j] + A[i][k] * B[k][j];
			}
		return R;
	}

	private double[][] getTranspose(double[][] M) {
		int rows = M[0].length; // yes, this is the opposite, because
		int columns = M.length; // we are preparing the transpose
		double[][] R = new double[rows][columns];
		for (int m = 0; m < rows; m++) {
			for (int n = 0; n < columns; n++) {
				R[m][n] = M[n][m];
			}
		}
		return R;
	}

	private double[][] getPseudoInverse(double[][] j) {
		Matrix<Float64>  M = Float64Matrix.valueOf(j);
		int rows = j[0].length; // yes, this is the opposite, because
		int columns = j.length; // we are preparing the inverse
		Matrix<Float64> iM = M.pseudoInverse();
		// M.inverse();
		double[][] r = new double[rows][columns];
		for (int m = 0; m < rows; m++) 
			for (int n = 0; n < columns; n++) 
				r[m][n] = iM.get(m, n).doubleValue();
		return r;
	}

	private double[][] getError(Vector2[] v1, Vector2[] v2) {
		double[][] error = new double[2][v1.length];
		for (int i = 0; i < v1.length; i++) {
			error[0][i] = v1[i].x - v2[i].x;
			error[1][i] = v1[i].y - v2[i].y;
		}
		return error;
	}

	private double[][] getJacobian(Vector2[] imagePoints, double jacobianDepth) {

		double[][] J = new double[2 * imagePoints.length][6];
		int currentRow = 0;
		for (int i = 0; i < imagePoints.length; i++) {
			double[][] row = getJacobianRow(imagePoints[i], jacobianDepth);
			J[currentRow] = row[0];
			J[currentRow + 1] = row[1];
			currentRow = currentRow + 2;
		}
		return J;
	}

	private double[][] getJacobianRow(Vector2 imagePoint, double jacobianDepth) {

		double x = imagePoint.x;
		double y = imagePoint.y;
		double z = jacobianDepth;
		double[][] jRow = { { -1 / z, 0d, x / z, x * z, -(1 + x * x), y },
			                { 0d, -1 / z, y / z, 1 + x * x, -x * y, -x * y } };
		return jRow;
	}

	@Override
	public JPanel getPanel() {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		compute(i);
		
	}

}
